#include "klt_object_tracking.h"

KLT_Object_Tracking::KLT_Object_Tracking(QWidget *parent)
    : CommonGraphicsView{parent}
{
    isShowLine = false;
    this->setWindowTitle("KLT稀疏光流实现对象跟踪");
    QPushButton *btn = new QPushButton(this);
    btn->setText("选择视频");
    connect(btn,&QPushButton::clicked,[=](){
        //选择视频
        path = QFileDialog::getOpenFileName(this,"请选择视频","/Users/yangwei/Downloads/",tr("Image Files(*.mp4 *.avi)"));
        qDebug()<<"视频路径："<<path;
        startKltTracking(path.toStdString().c_str());
    });
    //
    QButtonGroup * group = new QButtonGroup(this);
    QRadioButton * radioNo = new QRadioButton(this);
    radioNo->setText("否");
    radioNo->setChecked(true);
    QRadioButton *radioYes = new QRadioButton(this);
    radioYes->setText("是");
    group->addButton(radioNo,0);
    group->addButton(radioYes,1);

    radioNo->move(0,btn->y()+btn->height()+20);
    radioYes->move(radioNo->x()+radioNo->width()+20,btn->y()+btn->height()+20);
    connect(radioNo,&QRadioButton::clicked,[=](){
        isShowLine = false;//显示光流线
    });
    connect(radioYes,&QRadioButton::clicked,[=](){
        isShowLine = true;//不显示光流线
    });

}

void KLT_Object_Tracking::startKltTracking(const char* filePath){
    //【1】实例化VideoCapture并打开视频
    VideoCapture capture;//实例化视频捕获器
    capture.open(filePath);//打开视频文件（或摄像头）
    if(!capture.isOpened()){//检测文件是否打开，如果没打开直接退出
        qDebug()<<"无法打开视频";
        return;
    }

    Mat frame,gray;
    vector<Point2f> features;//检测出来的角点集合
    vector<Point2f> inPoints;//这个主要是为了画线用的
    vector<Point2f> fpts[2];//[0],存入的是是二维特征向量，[1]输出的二维特征向量
    Mat pre_frame,pre_gray;
    vector<uchar> status;//光流输出状态
    vector<float> err;//光流输出错误
    //【2】循环读取视频
    while(capture.read(frame)){//循环读取视频中每一帧的图像
        //【3】将视频帧图像转为灰度图
        cvtColor(frame,gray,COLOR_BGR2GRAY);//ps：角点检测输入要求单通道

        //【4】如果特征向量（角点）小于40个我们就重新执行角点检测
        if(fpts[0].size()<40){//如果小于40个角点就重新开始执行角点检测
            //执行角点检测
            goodFeaturesToTrack(gray,features,5000,0.01,10,Mat(),3,false,0.04);
            //【5】将检测到的角点放入fpts[0]中作为，光流跟踪的输入特征向量
            //将检测到的角点插入vector
            fpts[0].insert(fpts[0].begin(),features.begin(),features.end());
            inPoints.insert(inPoints.end(),features.begin(),features.end());
            qDebug()<<"角点检测执行完成，角点个数为："<<features.size();
        }else{
            qDebug()<<"正在跟踪...";
        }
        //【6】初始化的时候如果检测到前一帧为空，这个把当前帧的灰度图像给前一帧
        if(pre_gray.empty()){//如果前一帧为空就给前一帧赋值一次
            gray.copyTo(pre_gray);
        }

        //执行光流跟踪
        qDebug()<<"开始执行光流跟踪";
        //【7】执行光流跟踪，并将输出的特征向量放入fpts[1]中
        calcOpticalFlowPyrLK(pre_gray,gray,fpts[0],fpts[1],status,err);
        qDebug()<<"光流跟踪执行结束";
        //【8】遍历光流跟踪的输出特征向量，并得到距离和状态都符合预期的特征向量。让后将其重新填充到fpts[1]中备用
        int k =0;
        for(size_t i=0;i<fpts[1].size();i++){//循环遍历二维输出向量
            double dist = abs(fpts[0][i].x - fpts[1][i].x) + abs(fpts[0][i].y - fpts[1][i].y);//特征向量移动距离
            if(dist>2&&status[i]){//如果距离大于2，status=true（正常）
                inPoints[k] = inPoints[i];
                fpts[1][k++] = fpts[1][i];
            }
        }
        //【9】重置集合大小（由于有错误/不符合条件的输出特征向量），只拿状态正确的
        //重新设置集合大小
        inPoints.resize(k);
        fpts[1].resize(k);
        //【10】绘制光流线，这一步要不要都行
        //绘制光流线
        if(isShowLine){
            for(size_t i = 0;i<fpts[1].size();i++){
                line(frame,inPoints[i],fpts[1][i],Scalar(0,255,0),1,8,0);
                circle(frame, fpts[1][i], 2, Scalar(0, 0, 255), 2, 8, 0);
            }
        }

        qDebug()<<"特征向量的输入输出交换数据";
        //【11】交换特征向量的输入和输出，（循环往复/进入下一个循环），此时特征向量的值会递减
        std::swap(fpts[1],fpts[0]);//交换特征向量的输入和输出，此处焦点的总数量会递减

        //【12】将用于跟踪的角点绘制出来
        //将角点绘制出来
        for(size_t i = 0;i<fpts[0].size();i++){
            circle(frame,fpts[0][i],2,Scalar(0,0,255),2,8,0);
        }

        //【13】重置前一帧图像（每一个循环都要刷新）
        gray.copyTo(pre_gray);
        frame.copyTo(pre_frame);
        //【14】展示最终的效果
        imshow("frame",frame);
        int keyValue = waitKey(100);
        if(keyValue==27){//如果用户按ese键退出播放
            break;
        }
    }
}
